/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/helpSrc/distance_check.cpp
 * @Description  : DisCheck类用于检查路径和轨迹的有效性及距离差异是否超限。
 *                 包含初始化、重置、参数更新、路径添加等功能。
 *                 通过Allowed函数判断是否允许执行操作，并通过Trigger函数检查距离差异超限。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:49:20
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "distance_check.h"  

/**
 * @brief DisCheck类的构造函数
 *
 * 初始化DisCheck类的成员变量，并调用Reset函数进行初始化设置。
 */
DisCheck::DisCheck()
        : init_(false),
        enable_(false),
        open_(false),
        max_dis_(1.0f),
        use_theory_vel_(false) 
{
    Reset();
}
// void DisCheck::CycleRoutine()
// {
//     std::lock_guard<std::mutex> locker(m_mutex);
//     if (!Allowed())
//     {
//         return;
//     }
// }

// void DisCheck::Thread_fun()
// {
//     while (!m_quitMutex.try_lock_for(std::chrono::milliseconds(10)))
//     {
//         CycleRoutine();
//     }
// }
/**
 * @brief 清空检查缓冲区
 *
 * 该函数用于清空DisCheck类的检查缓冲区。
 *
 * 在执行清空操作前，会使用std::lock_guard对互斥锁mutex_进行加锁，以保证线程安全。
 * 接着调用Reset函数重置缓冲区。
 * 最后，输出调试信息，表示缓冲区已清空。
 */
void DisCheck::ClearCheckBuffer() 
{
    std::lock_guard<std::mutex> locker(mutex_);
    Reset();
    DEBUG_OUT("DisCheck ClearCheckBuffer ok");
}

/**
 * @brief 更新DisCheck类的参数
 *
 * 该函数从ROS参数服务器读取参数，并更新DisCheck类的成员变量。
 */
void DisCheck::UpdateParam()
{
    max_dis_ = ROSNode()->get_parameter_or("/agv_navigate_node/dis_check/max_dis_diff", 1.0f);
    use_theory_vel_ = ROSNode()->get_parameter_or("/agv_navigate_node/dis_check/use_theory_vel", false);
    open_ = ROSNode()->get_parameter_or("/agv_navigate_node/dis_check/open", true);

    DEBUG_OUT("DisCheck :UpdateParam ok;m_fMaxDis=" << max_dis_ << ",bUseTheoryVel=" << use_theory_vel_
                                                    << ",m_bOpen=" << open_);
}

/**
 * @brief 更新运行数据
 *
 * 更新从from_node到to_node的进度数据，并处理时间戳更新
 *
 * @param from_node 起始节点
 * @param to_node 目标节点
 * @param abs_progress 绝对进度值
 */
void DisCheck::UpdateProgRunData(USHORT from_node, USHORT to_node, float abs_progress)
{
    if (!Allowed()) {
        DEBUG_ERROR_OUT("error: DisCheck UpdateProgRunData failed,m_bOpen" << open_ 
                        << ",m_bInit:" << init_ << ",m_bEnable:" << enable_);
        return;
    }
    cur_run_data_.Update(from_node, to_node, abs_progress);

    // 更新周期时间
    float time_ms = time_stamp_.GetDiffMs();
    if (time_stamp_.IsNew(10)) {
        DEBUG_WARN_OUT("UpdateProgRunData: DisCheck timeStamp is too short (<10ms); time_ms=" << time_ms);
    } else if (time_stamp_.TimeOut(200)) {
        DEBUG_ERROR_OUT("UpdateProgRunData: DisCheck timeStamp is too long (>200ms); force set 50ms; time_ms=" << time_ms);
        time_ms = 50;
    }
    cur_run_data_.AccumulatedDis(time_ms);
    time_stamp_.Update();
    
    DEBUG_OUT("UpdateProgRunData: DisCheck timeStamp updated once; last time_ms=" << time_ms);
}

/**
 * @brief 添加新的路径
 *
 * 在DisCheck对象中添加一个新的路径和轨迹。
 *
 * @param path 指向Path对象的指针，包含路径信息
 * @param traj 指向Trajectory对象的指针，包含轨迹信息
 *
 * @return 如果添加成功，返回true；否则返回false
 */
bool DisCheck::AddNewPath(Path *path, Trajectory *traj)
{
    std::lock_guard<std::mutex> locker(mutex_);
    time_stamp_.Update();
    DEBUG_OUT("AddNewPath: DisCheck timeStamp updated once");
    if (!Allowed()) {
        DEBUG_ERROR_OUT("Error: DisCheck AddNewPath failed, open=" << open_ 
                        << ", init=" << init_ 
                        << ", enable=" << enable_);
        return false;
    }

    // #1 轨迹对象不存在
    if (!path || !traj) {
        DEBUG_OUT("AddNewPath: path or traj is null");
        return false;
    }

    // #2 轨迹类型不匹配
    int traj_type = traj->GetType();
    bool traj_type_valid = (int)TrajType::BEZIER_TRAJ == traj_type || 
                           (int)TrajType::LINE_TRAJ == traj_type || 
                           (int)TrajType::SIDE_TRAJ == traj_type;

    if (!traj_type_valid) {
        DEBUG_OUT("AddNewPath: TrajType is not valid: traj_type=" << traj_type);
        return false;
    }

    // #4 启终点相同
    USHORT start_id = path->start_node_;
    USHORT end_id = path->end_node_;
    
    if (start_id == end_id) {
        DEBUG_OUT("AddNewPath: node id is the same; start_id=" << start_id 
                  << ", end_id=" << end_id);
        return false;
    }

    Reset();                          // 清除数据
    float range = traj->GetRange();   // 当前要运行的长度(如果时路段中间，则此长度<path.siez)
    float size = path->Size();        // 地图上整条路线长度
    DEBUG_OUT("AddNewPath: start_id=" << start_id 
              << ", end_id=" << end_id 
              << ", range(used)=" << range 
              << ", size(path)=" << size);
    target_run_data_.Update(start_id, end_id, range);
    return true;
}

/**
 * @brief 设置DisCheck的启用状态
 *
 * 该函数用于设置DisCheck的启用状态。当启用状态改变时，会相应地调用Reset或UpdateParam函数进行初始化或更新参数。
 *
 * @param enable 启用状态，true表示启用，false表示禁用
 */
void DisCheck::SetEnable(bool enable)
{
    std::lock_guard<std::mutex> locker(mutex_);
    enable_ = enable;
    
    if (!enable_) {
        Reset();
    } else {
        UpdateParam();
    }
    
    DEBUG_OUT("DisCheck SetEnable: enable=" << enable);
}

/**
 * @brief 检查是否允许
 *
 * 检查三个标志位 init_、enable_ 和 open_ 是否都为 true，
 * 如果都为 true，则返回 true，表示允许；否则返回 false。
 *
 * @return 如果允许，则返回 true；否则返回 false。
 */
bool DisCheck::Allowed()
{
    return init_ && enable_ && open_;
}

/**
 * @brief 初始化DisCheck对象
 *
 * 如果DisCheck对象已经初始化过，则直接返回true；否则，进行初始化操作。
 *
 * @return 如果初始化成功，返回true；否则返回false。
 */
bool DisCheck::Init()
{
    if (init_) {
        DEBUG_OUT("DisCheck init finished before");
        return true;
    }

    // m_quitMutex.lock();
    //  m_thread = std::thread(std::bind(&DisCheck::Thread_fun, this)); // std::placeholders::_1), this);
    UpdateParam();
    DEBUG_OUT("DisCheck init finished first");
    init_ = true;
    return true;
}

/**
 * @brief 重置DisCheck对象的状态
 *
 * 该函数用于清空当前运行数据和目标运行数据，将DisCheck对象重置为初始状态。
 */
void DisCheck::Reset()
{
    cur_run_data_.Clear();
    target_run_data_.Clear();
}

/**
 * @brief 检查两个节点是否有效且不同
 *
 * 检查两个节点 ID 是否有效，并且这两个节点不是同一个节点。
 *
 * @param from_id 起始节点的 ID
 * @param to_id 目标节点的 ID
 *
 * @return 如果两个节点有效且不同，则返回 true；否则返回 false
 */
bool DisCheck::NodeValid(USHORT from_id, USHORT to_id)
{
   return !NodeSame(from_id, to_id) && IdValid(from_id) && IdValid(to_id);
}

/**
 * @brief 判断两个节点是否相同
 *
 * 根据给定的两个节点的ID，判断这两个节点是否相同。
 *
 * @param id1 第一个节点的ID
 * @param id2 第二个节点的ID
 *
 * @return 如果两个节点的ID相同，则返回true；否则返回false
 */
bool DisCheck::NodeSame(USHORT id1, USHORT id2)
{
    return id1 == id2;
}

/**
 * @brief 触发检查距离是否超限的函数
 *
 * 该函数检查当前运行数据和目标运行数据的节点有效性、匹配性以及距离差是否超限。
 * 如果满足条件，则触发超限报警，并返回true；否则返回false。
 *
 * @param msg 用于存储超限报警信息的字符串引用
 * @return 如果触发超限报警，则返回true；否则返回false
 */
bool DisCheck::Trigger(std::string &msg)
{
    if (!Allowed()) {
        return false;
    }
    
    USHORT from_id1 = cur_run_data_.from_node, from_id2 = target_run_data_.from_node;
    USHORT to_id1 = cur_run_data_.to_node, to_id2 = target_run_data_.to_node;
    
    bool run_valid = NodeValid(from_id1, to_id1);
    bool target_valid = NodeValid(from_id2, to_id2);
    bool match = NodeSame(from_id1, from_id2) && NodeSame(to_id1, to_id2);
    
    if (!run_valid || !target_valid || !match)
    {
        DEBUG_WARN_OUT("Failed: try to check dis overLimit; curNodeValid="
                       << run_valid << ", targetNodeValid=" << target_valid
                       << ", match=" << match
                       << ", from_id1=" << from_id1 << ", to_id1=" << to_id1
                       << ", from_id2=" << from_id2 << ", to_id2=" << to_id2);
        return false;
    }
    // 注意：此处都是标量(正值)比较
    float cur_size = use_theory_vel_ ? cur_run_data_.cur_theory_size : cur_run_data_.cur_real_size;
    float diff_size = cur_size - target_run_data_.total_size;
    bool trigger = diff_size > max_dis_;
    if (trigger) {
        DEBUG_WARN_OUT("OverLimitTrigger: "
                       << "use_theory_vel=" << use_theory_vel_ 
                       << ", cur_real_size=" << cur_run_data_.cur_real_size 
                       << ", cur_theory_size=" << cur_run_data_.cur_theory_size
                       << ", cur_size=" << cur_size 
                       << ", total_size=" << target_run_data_.total_size 
                       << ", diff_size=" << diff_size 
                       << ", max_dis=" << max_dis_);
    }

    msg = "fromNodeId:" + std::to_string(from_id1) + ", toNodeId:" + std::to_string(to_id1) + ", overLength:" + std::to_string(diff_size);
    return trigger;
}

/**
 * @brief 检查ID是否有效
 *
 * 检查给定的ID是否有效。如果ID不是65535，则认为ID有效。
 *
 * @param id 需要检查的ID
 * @return 如果ID有效，返回true；否则返回false
 */
bool DisCheck::IdValid(USHORT id)
{
    return id != 65535;
}